import numpy as np

def iterate_matrix(matrix):
    parameters = []
    for row in matrix:
        for parameter in row:
            parameters.append(parameter)
    return parameters

def camera_to_world(R, T):
    # 创建一个4x4的单位矩阵
    world_matrix = np.eye(4)
    
    # 将旋转矩阵和平移向量填充到世界坐标系矩阵中
    world_matrix[:3, :3] = R
    world_matrix[:3, 3] = T
    
    return world_matrix

def multiply_martices(world_matrix,V):
    result = np.dot(world_matrix,V)
    return result

# 示例：已知相机的旋转矩阵R和平移向量T
R = np.array([[0.866, -0.5, 0],
              [0.5, 0.866, 0],
              [0, 0, 1]])
T = np.array([1, 2, 3])

V = np.array([[8.416197871731415e-05],
             [0.0004867154551824521],
             [0],
             [1]])


# 计算相机在世界坐标系下的坐标
world_matrix = camera_to_world(R, T)
result = multiply_martices(world_matrix,V)
print("相机在世界坐标系下的坐标:\n", world_matrix)
print("转换后的坐标系：",result)
parameters = iterate_matrix(result)
print("矩阵中的每个参数：\n",parameters)
print("X坐标(单位mm)：",parameters[0])
print("Y坐标(单位mm)：",parameters[1])
print("Z坐标(单位mm)：",parameters[2])

